
// import haxe.ds.Array;
// import oimo.collision.geometry.*;
// import oimo.collision.narrowphase.detector.*;
// import oimo.common.Mat3;
// import oimo.common.Transform;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { Transform } from "../../../../common/Transform";
import { Vec3 } from "../../../../common/Vec3";
import { CachedDetectorData } from "../CachedDetectorData";
import { GjkCache } from "./GjkCache";
import { M } from "../../../../common/M";
import { RayCastHit } from "../../../geometry/RayCastHit";
import { GjkEpaLog } from "./GjkEpaLog";
import { SimplexUtil } from "./SimplexUtil";
import { EpaTriangle } from "./EpaTriangle";
import { EpaVertex } from "./EpaVertex";
import { ConvexGeometry } from "../../../geometry/ConvexGeometry";
import { GjkEpaResultState } from "./GjkEpaResultState";
import { EpaPolyhedron } from "./EpaPolyhedron";

/**
 * GJK algorithm and EPA for narrow-phase collision detection.
 */

export class GjkEpa {
	c1: ConvexGeometry;
	c2: ConvexGeometry;
	tf1: Transform;
	tf2: Transform;

	static instance: GjkEpa = new GjkEpa();

	// ------------------------------------------------------------- for GJK

	// simplex
	s: Array<Vec3>;
	simplexSize: number;

	// witness points
	w1: Array<Vec3>;
	w2: Array<Vec3>;

	tempVec3s: Array<Vec3>;
	tempTransform: Transform;

	// direction
	dir: Vec3;

	// closest point
	closest: Vec3;

	// base directions used to expand simplex
	baseDirs: Array<Vec3>;

	// for convex casting
	tl1: Vec3;
	tl2: Vec3;
	rayX: Vec3;
	rayR: Vec3;

	// ------------------------------------------------------------- for EPA

	depth: number;
	polyhedron: EpaPolyhedron;

	// ------------------------------------------------------------- public vars

	/**
	 * Computed closest point of the first geometry in world coordinate system.
	 */
	closestPoint1: Vec3;

	/**
	 * Computed closest point of the second geometry in world coordinate system.
	 */
	closestPoint2: Vec3;

	/**
	 * Computed distance between two geometries. This value may be negative if two
	 * geometries are overlapping.
	 */
	distance: number;

	/**
	 * Default constructor. Consider using `GjkEpa.getInstance` instead of creating a new
	 * instance.
	 */
	constructor() {
		this.s = new Array<Vec3>(4);
		this.w1 = new Array<Vec3>(4);
		this.w2 = new Array<Vec3>(4);

		this.baseDirs = new Array<Vec3>(3);
		this.baseDirs[0] = new Vec3(1, 0, 0);
		this.baseDirs[1] = new Vec3(0, 1, 0);
		this.baseDirs[2] = new Vec3(0, 0, 1);

		this.tl1 = new Vec3();
		this.tl2 = new Vec3();
		this.rayX = new Vec3();
		this.rayR = new Vec3();
		this.tempTransform = new Transform();

		for (let i = 0; i < 4; ++i) {
			this.s[i] = new Vec3();
			this.w1[i] = new Vec3();
			this.w2[i] = new Vec3();
		}

		this.dir = new Vec3();
		this.closest = new Vec3();

		this.closestPoint1 = new Vec3();
		this.closestPoint2 = new Vec3();

		this.polyhedron = new EpaPolyhedron();
	}

	// --- ---

	public computeClosestPointsImpl(c1: ConvexGeometry, c2: ConvexGeometry, tf1: Transform, tf2: Transform, cache: CachedDetectorData, useEpa: boolean): number {
		GjkEpaLog.log("----------- GJK begin -----------");

		this.c1 = c1;
		this.c2 = c2;
		this.tf1 = tf1;
		this.tf2 = tf2;

		// simplex
		var s: Array<Vec3> = this.s;

		// witness points
		var w1: Array<Vec3> = this.w1;
		var w2: Array<Vec3> = this.w2;

		var closest: Vec3 = this.closest;

		var dir: Vec3 = this.dir;

		if (cache != null) {
			if (cache._gjkCache == null) cache._gjkCache = new GjkCache();
			this.loadCache(cache._gjkCache);
		} else {
			dir.zero();
		}

		if (dir.lengthSq() == 0) {
			// compute the first vertex of the simplex
			tf2._position.subTo(tf1._position, dir);
			if (dir.lengthSq() < 1e-6) {
				dir.init(1, 0, 0);
			}
		}

		this.simplexSize = 0;
		this.computeSupportingVertex();
		this.simplexSize = 1;

		// loop count and max iteration of the loop
		var count: number = 0;
		var max: number = 40;

		var eps: number = 1e-4;
		var eps2: number = eps * eps;

		while (count < max) {
			// project the origin to the simplex and compute index of voronoi region of the origin.
			var v: number = 0;
			GjkEpaLog.log('simplex size: $simplexSize');
			GjkEpaLog.log('projecting the origin to the simplex...');
			switch (this.simplexSize) {
				case 1:
					closest.copyFrom(s[0]);
					GjkEpaLog.log('${s[0]}');
					v = 1;
					break;
				case 2:
					v = SimplexUtil.projectOrigin2(s[0], s[1], closest);
					GjkEpaLog.log('${s[0]}');
					GjkEpaLog.log('${s[1]}');
					break;
				case 3:
					v = SimplexUtil.projectOrigin3(s[0], s[1], s[2], closest);
					GjkEpaLog.log('${s[0]}');
					GjkEpaLog.log('${s[1]}');
					GjkEpaLog.log('${s[2]}');
					break;
				case 4:
					v = SimplexUtil.projectOrigin4(s[0], s[1], s[2], s[3], closest);
					GjkEpaLog.log('${s[0]}');
					GjkEpaLog.log('${s[1]}');
					GjkEpaLog.log('${s[2]}');
					GjkEpaLog.log('${s[3]}');
					break;
			}

			// check if the origin is touching or inside the simplex
			if (closest.lengthSq() < eps2) {
				if (!useEpa) {
					this.distance = 0;
					return GjkEpaResultState._SUCCEEDED;
				}

				// make the simplex to be a tetrahedron for EPA computation
				switch (this.simplexSize) {
					case 1:
						this.pointToTetrahedron();
						GjkEpaLog.log("point -> tetrahedron");
						break;
					case 2:
						this.lineToTetrahedron();
						GjkEpaLog.log("line -> tetrahedron");
						break;
					case 3:
						this.triangleToTetrahedron();
						GjkEpaLog.log("triangle -> tetrahedron");
						break;
				}
				if (this.simplexSize == 4) {
					var epaState: number = this.computeDepth(c1, c2, tf1, tf2, s, w1, w2);
					if (epaState != GjkEpaResultState._SUCCEEDED) {
						this.distance = 0;
						return epaState;
					}
					this.distance = -this.depth;
					return GjkEpaResultState._SUCCEEDED;
				}
				// failed to make a tetrahedron
				this.distance = 0;
				return GjkEpaResultState._GJK_FAILED_TO_MAKE_TETRAHEDRON;
			}

			GjkEpaLog.log('projected origin: $v');

			// shrink the simplex according to the voronoi index of the origin
			this.shrinkSimplex(v);

			// compute the next vertex
			dir.copyFrom(closest).negateEq();
			this.computeSupportingVertex();

			if (dir.lengthSq() < eps2) {
				throw M.error("!?"); // this should never be happen
			}

			var d1: number = closest.dot(dir);
			var d2: number = s[this.simplexSize].dot(dir);

			GjkEpaLog.log('n: $simplexSize, prev: $closest, current: ${s[simplexSize]}, dir: $dir, iteration: $count, d2 - d1: ${d2 - d1}');

			if (d2 - d1 < eps2) { // terminate GJK; no improvement
				this.interpolateClosestPoints();

				GjkEpaLog.log("iteration: " + count);

				this.distance = closest.length(); // no improvement

				if (cache != null && cache._gjkCache != null) {
					this.saveCache(cache._gjkCache);
				}

				return GjkEpaResultState._SUCCEEDED;
			}

			this.simplexSize++;
			count++;
		}

		GjkEpaLog.log("GJK failed: did not converge");
		return GjkEpaResultState._GJK_DID_NOT_CONVERGE;
	}

	// `c1` can be null
	public convexCastImpl(c1: ConvexGeometry, c2: ConvexGeometry, tf1: Transform, tf2: Transform, tl1: Vec3, tl2: Vec3, hit: RayCastHit): boolean {
		GjkEpaLog.log("----------- GJK convex casting begin -----------");

		this.c1 = c1;
		this.c2 = c2;
		this.tf1 = tf1;
		this.tf2 = tf2;

		// simplex
		var s: Array<Vec3> = this.s;

		// witness points
		var w1: Array<Vec3> = this.w1;
		var w2: Array<Vec3> = this.w2;

		var closest: Vec3 = this.closest;

		var dir: Vec3 = this.dir;

		// compute the first vertex of the simplex
		tf2._position.subTo(tf1._position, dir);
		if (dir.lengthSq() < 1e-6) {
			dir.init(1, 0, 0);
		}

		this.simplexSize = 0;
		this.computeConvexCastSupportingVertex();
		this.simplexSize = 1;

		// loop count and max iteration of the loop
		var count: number = 0;
		var max: number = 40;

		var lambda: number = 0.0;
		var rayX: Vec3 = this.rayX; // origin
		var rayR: Vec3 = this.rayR; // relative translation
		rayX.zero();
		rayR.copyFrom(tl2).subEq(tl1);

		var eps: number = 1e-4;
		var eps2: number = eps * eps;

		while (count < max) {
			// project the origin to the simplex and compute index of voronoi region of the origin.
			var v: number = 0;
			GjkEpaLog.log('simplex size: $simplexSize');
			GjkEpaLog.log('projecting the origin to the simplex...');
			GjkEpaLog.log('x: $rayX');
			GjkEpaLog.log('lambda: $lambda');
			switch (this.simplexSize) {
				case 1:
					closest.copyFrom(s[0]);
					GjkEpaLog.log('${s[0]}');
					v = 1;
					break;
				case 2:
					v = SimplexUtil.projectOrigin2(s[0], s[1], closest);
					GjkEpaLog.log('${s[0]}');
					GjkEpaLog.log('${s[1]}');
					break;
				case 3:
					v = SimplexUtil.projectOrigin3(s[0], s[1], s[2], closest);
					GjkEpaLog.log('${s[0]}');
					GjkEpaLog.log('${s[1]}');
					GjkEpaLog.log('${s[2]}');
					break;
				case 4:
					v = SimplexUtil.projectOrigin4(s[0], s[1], s[2], s[3], closest);
					GjkEpaLog.log('${s[0]}');
					GjkEpaLog.log('${s[1]}');
					GjkEpaLog.log('${s[2]}');
					GjkEpaLog.log('${s[3]}');
					break;
			}

			GjkEpaLog.log('projected origin: pos = $closest, voronoi index = $v');

			// shrink the simplex according to the voronoi index of the origin
			this.shrinkSimplex(v);

			// check if the origin is touching or inside the simplex
			if (closest.lengthSq() < eps2) {
				if (lambda == 0 || this.simplexSize == 4) {
					GjkEpaLog.log("overlapping... closest: " + closest);
					hit.fraction = lambda;
					return false; // overlapping
				}

				this.interpolateClosestPoints();

				hit.fraction = lambda;
				hit.normal.copyFrom(dir).normalize(); // previous dir
				hit.position.copyFrom(this.closestPoint1).addScaledEq(tl1, lambda);
				GjkEpaLog.log("GJK convex cast succeeded");
				return true;
			}

			// compute the next vertex
			dir.copyFrom(closest).negateEq();
			this.computeConvexCastSupportingVertex();
			s[this.simplexSize].subEq(rayX); // translate origin

			if (dir.lengthSq() < eps2) {
				throw M.error("!?"); // this should never be happen
			}

			// n is the normal at the vertex p
			var p: Vec3 = s[this.simplexSize];
			var n: Vec3 = dir;
			GjkEpaLog.log('new vertex p = $p');
			GjkEpaLog.log('normal n = $n');
			GjkEpaLog.log('ray dir r = $rayR');

			// check if a part of the ray can be rejected
			var pn: number = p.dot(n);
			GjkEpaLog.log('p dot n = $pn');
			if (pn < 0) {
				// check if entire the ray can be rejected
				if (rayR.dot(n) >= 0) {
					GjkEpaLog.log("rejected [0");
					return false;
				}
				var dLambda: number = pn / rayR.dot(n);
				lambda += dLambda;
				if (lambda >= 1) {
					GjkEpaLog.log("rejected 1]");
					return false;
				}
				GjkEpaLog.log("advanced: " + dLambda);
				rayX.addScaledEq(rayR, dLambda);

				// translate the simplex
				for (let i = 0; i < this.simplexSize + 1; ++i) {
					s[i].addScaledEq(rayR, -dLambda);
				}
			} else {
				GjkEpaLog.log("ray did not advance");
			}

			// do not add new vertex to the simplex if already exists
			var duplicate: boolean = false;
			for (let i = 0; i < this.simplexSize; ++i) {
				var dx: number = s[i].x - s[this.simplexSize].x;
				var dy: number = s[i].y - s[this.simplexSize].y;
				var dz: number = s[i].z - s[this.simplexSize].z;
				if (dx * dx + dy * dy + dz * dz < eps2) {
					duplicate = true;
					GjkEpaLog.log('duplicate vertex ${s[i]} and ${s[simplexSize]}');
					break;
				}
			}
			if (!duplicate) {
				GjkEpaLog.log('added ${s[simplexSize]}');
				this.simplexSize++;
			}

			count++;

			GjkEpaLog.log('iteration: $count');
		}

		GjkEpaLog.log("GJK convex cast failed: did not converge");
		return false;
	}

	public interpolateClosestPoints(): void {
		switch (this.simplexSize) {
			case 1: {
				this.closestPoint1.copyFrom(this.w1[0]);
				this.closestPoint2.copyFrom(this.w2[0]);
			}
				break;
			case 2: {
				let c = this.closest;
				let s01: Vec3 = this.s[1].sub(this.s[0]);
				let invDet: number = M.vec3_dot(s01, s01);
				if (invDet != 0) invDet = 1 / invDet;
				let s0c: Vec3 = c.sub(this.s[0]);
				let t: number = M.vec3_dot(s0c, s01) * invDet;

				// compute closest points
				let diff: Vec3 = this.w1[1].sub(this.w1[0]);
				let cp1 = M.vec3_addRhsScaled(w10, diff, t);

				this.w2[1].subTo(this.w2[0], diff);
				let cp2 = M.vec3_addRhsScaled(w20, diff, t);

				this.closestPoint1.copyFrom(cp1);
				this.closestPoint2.copyFrom(cp2);
			}
				break;
			case 3: {
				let c = this.closest.clone();
				var s0: Vec3 = new Vec3();
				var w10: Vec3 = new Vec3();
				var w20: Vec3 = new Vec3();
				var s1: Vec3 = new Vec3();
				var w11: Vec3 = new Vec3();
				var w21: Vec3 = new Vec3();
				var s2: Vec3 = new Vec3();
				var w12: Vec3 = new Vec3();
				var w22: Vec3 = new Vec3();
				M.vec3_fromVec3(s0, this.s[0]); M.vec3_fromVec3(w10, this.w1[0]); M.vec3_fromVec3(w20, this.w2[0]);
				M.vec3_fromVec3(s1, this.s[1]); M.vec3_fromVec3(w11, this.w1[1]); M.vec3_fromVec3(w21, this.w2[1]);
				M.vec3_fromVec3(s2, this.s[2]); M.vec3_fromVec3(w12, this.w1[2]); M.vec3_fromVec3(w22, this.w2[2]);

				var s01: Vec3 = new Vec3();
				var s02: Vec3 = new Vec3();
				var s0c: Vec3 = new Vec3();
				M.vec3_sub(s01, s1, s0);
				M.vec3_sub(s02, s2, s0);
				M.vec3_sub(s0c, c, s0);

				var d11: number = M.vec3_dot(s01, s01);
				var d12: number = M.vec3_dot(s01, s02);
				var d22: number = M.vec3_dot(s02, s02);
				var d1c: number = M.vec3_dot(s01, s0c);
				var d2c: number = M.vec3_dot(s02, s0c);
				var invDet: number = d11 * d22 - d12 * d12;
				if (invDet != 0) invDet = 1 / invDet;
				let s: number = (d1c * d22 - d2c * d12) * invDet;
				var t: number = (-d1c * d12 + d2c * d11) * invDet;

				// compute closest points
				var diff: Vec3 = new Vec3();
				var cp1: Vec3 = new Vec3();
				var cp2: Vec3 = new Vec3();

				M.vec3_sub(diff, w11, w10);
				M.vec3_addRhsScaledTo(cp1, w10, diff, s);
				M.vec3_sub(diff, w12, w10);
				M.vec3_addRhsScaledEQ(cp1, cp1, diff, t);

				M.vec3_sub(diff, w21, w20);
				M.vec3_addRhsScaledTo(cp2, w20, diff, s);
				M.vec3_sub(diff, w22, w20);
				M.vec3_addRhsScaledEQ(cp2, cp2, diff, t);

				M.vec3_toVec3(this.closestPoint1, cp1);
				M.vec3_toVec3(this.closestPoint2, cp2);
			}
				break;
			default:
				throw M.error("!?");
		}
	}

	public loadCache(gjkCache: GjkCache): void {
		// copy simplex data from the cache
		this.dir.copyFrom(gjkCache.prevClosestDir);
	}

	public saveCache(gjkCache: GjkCache): void {
		// set GJK cache for the next computation
		gjkCache.prevClosestDir.copyFrom(this.closest).negateEq();
	}

	///@:extern
	/*inline*/ public shrinkSimplex(vertexBits: number): void {
		this.simplexSize = vertexBits;
		this.simplexSize = (this.simplexSize & 5) + (this.simplexSize >> 1 & 5);
		this.simplexSize = (this.simplexSize & 3) + (this.simplexSize >> 2 & 3);
		switch (vertexBits) {
			//case 0: // do nothing
			//case 1: // do nothing
			case 2: // 0 <- 1
				this.s[0].copyFrom(this.s[1]);
				this.w1[0].copyFrom(this.w1[1]);
				this.w2[0].copyFrom(this.w2[1]);
				break;
			//case 3: // do nothing
			case 4: // 0 <- 2
				this.s[0].copyFrom(this.s[2]);
				this.w1[0].copyFrom(this.w1[2]);
				this.w2[0].copyFrom(this.w2[2]);
				break;
			case 5: // 1 <- 2
				this.s[1].copyFrom(this.s[2]);
				this.w1[1].copyFrom(this.w1[2]);
				this.w2[1].copyFrom(this.w2[2]);
				break;
			case 6: // 0 <- 2
				this.s[0].copyFrom(this.s[2]);
				this.w1[0].copyFrom(this.w1[2]);
				this.w2[0].copyFrom(this.w2[2]);
				break;
			//case 7: // do nothing
			case 8: // 0 <- 3
				this.s[0].copyFrom(this.s[3]);
				this.w1[0].copyFrom(this.w1[3]);
				this.w2[0].copyFrom(this.w2[3]);
				break;
			case 9: // 1 <- 3
				this.s[1].copyFrom(this.s[3]);
				this.w1[1].copyFrom(this.w1[3]);
				this.w2[1].copyFrom(this.w2[3]);
				break;
			case 10: // 0 <- 3
				this.s[0].copyFrom(this.s[3]);
				this.w1[0].copyFrom(this.w1[3]);
				this.w2[0].copyFrom(this.w2[3]);
				break;
			case 11: // 2 <- 3
				this.s[2].copyFrom(this.s[3]);
				this.w1[2].copyFrom(this.w1[3]);
				this.w2[2].copyFrom(this.w2[3]);
				break;
			case 12: // 0 <- 2, 1 <- 3
				this.s[0].copyFrom(this.s[2]);
				this.w1[0].copyFrom(this.w1[2]);
				this.w2[0].copyFrom(this.w2[2]);
				this.s[1].copyFrom(this.s[3]);
				this.w1[1].copyFrom(this.w1[3]);
				this.w2[1].copyFrom(this.w2[3]);
				break;
			case 13: // 1 <- 3
				this.s[1].copyFrom(this.s[3]);
				this.w1[1].copyFrom(this.w1[3]);
				this.w2[1].copyFrom(this.w2[3]);
				break;
			case 14: // 0 <- 3
				this.s[0].copyFrom(this.s[3]);
				this.w1[0].copyFrom(this.w1[3]);
				this.w2[0].copyFrom(this.w2[3]);
				break;
			//case 15: // do nothing
		}
	}


	private computeSupportingVertex(): void {
		this.computeWitnessPoint1(false);
		this.computeWitnessPoint2(false);
		this.s[this.simplexSize].copyFrom(this.w1[this.simplexSize]).subEq(this.w2[this.simplexSize]);
	}


	private computeConvexCastSupportingVertex(): void {
		if (this.c1 != null) {
			this.computeWitnessPoint1(true);
		} else {
			M.vec3_toVec3(this.w1[this.simplexSize], this.tf1._position);
		}
		this.computeWitnessPoint2(true);
		this.s[this.simplexSize].copyFrom(this.w1[this.simplexSize]).subEq(this.w2[this.simplexSize]);
	}

	public computeWitnessPoint1(addMargin: boolean): void {
		var tmp: Vec3 = new Vec3();
		var idir: Vec3 = new Vec3();
		M.vec3_fromVec3(idir, this.dir);

		// compute local dir
		var ldir1: Vec3 = new Vec3();
		M.vec3_mulMat3Transposed(ldir1, idir, this.tf1._rotation);

		// compute local witness point
		var iw1: Vec3 = new Vec3();
		M.vec3_toVec3(this.dir, ldir1);
		this.c1.computeLocalSupportingVertex(this.dir, this.w1[this.simplexSize]);
		if (addMargin) {
			this.dir.normalize();
			this.w1[this.simplexSize].addScaledEq(this.dir, this.c1._gjkMargin);
		}

		// compute world witness point
		M.vec3_fromVec3(tmp, this.w1[this.simplexSize]);
		M.vec3_mulMat3(iw1, tmp, this.tf1._rotation);
		M.vec3_add(iw1, iw1, this.tf1._position);
		M.vec3_toVec3(this.w1[this.simplexSize], iw1);

		M.vec3_toVec3(this.dir, idir);
	}

	public computeWitnessPoint2(addMargin: boolean): void {
		var tmp: Vec3 = new Vec3();
		var idir: Vec3 = new Vec3();
		M.vec3_fromVec3(idir, this.dir);

		// compute local dir
		var ldir2: Vec3 = new Vec3();
		M.vec3_mulMat3Transposed(ldir2, idir, this.tf2._rotation);
		M.vec3_negate(ldir2, ldir2);

		// compute local witness point
		var iw2: Vec3 = new Vec3();
		M.vec3_toVec3(this.dir, ldir2);
		this.c2.computeLocalSupportingVertex(this.dir, this.w2[this.simplexSize]);
		if (addMargin) {
			this.dir.normalize();
			this.w2[this.simplexSize].addScaledEq(this.dir, this.c2._gjkMargin);
		}

		// compute world witness point
		M.vec3_fromVec3(tmp, this.w2[this.simplexSize]);
		M.vec3_mulMat3(iw2, tmp, this.tf2._rotation);
		M.vec3_add(iw2, iw2, this.tf2._position);
		M.vec3_toVec3(this.w2[this.simplexSize], iw2);

		M.vec3_toVec3(this.dir, idir);
	}

	///@:extern
	/*inline*/ public pointToTetrahedron(): void {
		for (let i = 0; i < 3; ++i) {
			this.dir.copyFrom(this.baseDirs[i]);

			this.computeSupportingVertex();
			this.simplexSize++;
			this.lineToTetrahedron();
			if (this.simplexSize == 4) break;
			this.simplexSize--;

			this.dir.negateEq();

			this.computeSupportingVertex();
			this.simplexSize++;
			this.lineToTetrahedron();
			if (this.simplexSize == 4) break;
			this.simplexSize--;
		}
	}

	///@:extern
	/*inline*/ public lineToTetrahedron(): void {
		var oldDir: Vec3 = new Vec3();
		M.vec3_fromVec3(oldDir, this.dir);

		var s0: Vec3 = new Vec3();
		var s1: Vec3 = new Vec3();
		var lineDir: Vec3 = new Vec3();
		M.vec3_fromVec3(s0, this.s[0]);
		M.vec3_fromVec3(s1, this.s[1]);
		M.vec3_sub(lineDir, s0, s1);
		for (let i = 0; i < 3; ++i) {
			var baseDir: Vec3 = new Vec3();
			M.vec3_fromVec3(baseDir, this.baseDirs[i]);
			var newDir: Vec3 = new Vec3();
			M.vec3_cross(newDir, lineDir, baseDir);
			M.vec3_toVec3(this.dir, newDir);

			this.computeSupportingVertex();
			this.simplexSize++;
			this.triangleToTetrahedron();
			if (this.simplexSize == 4) break;
			this.simplexSize--;

			this.dir.negateEq();

			this.computeSupportingVertex();
			this.simplexSize++;
			this.triangleToTetrahedron();
			if (this.simplexSize == 4) break;
			this.simplexSize--;
		}

		M.vec3_toVec3(this.dir, oldDir);
	}

	///@:extern
	/*inline*/ public triangleToTetrahedron(): void {
		var oldDir: Vec3 = new Vec3();
		M.vec3_fromVec3(oldDir, this.dir);

		do {
			var s0: Vec3 = new Vec3();
			var s1: Vec3 = new Vec3();
			var s2: Vec3 = new Vec3();
			var s01: Vec3 = new Vec3();
			var s02: Vec3 = new Vec3();
			M.vec3_fromVec3(s0, this.s[0]);
			M.vec3_fromVec3(s1, this.s[1]);
			M.vec3_fromVec3(s2, this.s[2]);
			M.vec3_sub(s01, s1, s0);
			M.vec3_sub(s02, s2, s0);
			var n: Vec3 = new Vec3();
			M.vec3_cross(n, s01, s02);
			M.vec3_toVec3(this.dir, n);

			this.computeSupportingVertex();
			this.simplexSize++;
			if (this.isValidTetrahedron()) break;
			this.simplexSize--;

			this.dir.negateEq();

			this.computeSupportingVertex();
			this.simplexSize++;
			if (this.isValidTetrahedron()) break;
			this.simplexSize--;
		} while (false);

		M.vec3_toVec3(this.dir, oldDir);
	}

	/*inline*/ public isValidTetrahedron(): boolean {
		var e00: number = this.s[1].x - this.s[0].x;
		var e01: number = this.s[1].y - this.s[0].y;
		var e02: number = this.s[1].z - this.s[0].z;
		var e10: number = this.s[2].x - this.s[0].x;
		var e11: number = this.s[2].y - this.s[0].y;
		var e12: number = this.s[2].z - this.s[0].z;
		var e20: number = this.s[3].x - this.s[0].x;
		var e21: number = this.s[3].y - this.s[0].y;
		var e22: number = this.s[3].z - this.s[0].z;
		var det: number = e00 * (e11 * e22 - e12 * e21) - e01 * (e10 * e22 - e12 * e20) + e02 * (e10 * e21 - e11 * e20);
		return det > 1e-12 || det < -1e-12;
	}

	// EPA

	public computeDepth(convex1: ConvexGeometry, convex2: ConvexGeometry, tf1: Transform, tf2: Transform, initialPolyhedron: Array<Vec3>, initialPolyhedron1: Array<Vec3>, initialPolyhedron2: Array<Vec3>): number {
		GjkEpaLog.log("----------- EPA begin ----------- ");

		this.polyhedron._clear();
		if (!this.polyhedron._init(
			this.polyhedron._pickVertex().init(initialPolyhedron[0], initialPolyhedron1[0], initialPolyhedron2[0]),
			this.polyhedron._pickVertex().init(initialPolyhedron[1], initialPolyhedron1[1], initialPolyhedron2[1]),
			this.polyhedron._pickVertex().init(initialPolyhedron[2], initialPolyhedron1[2], initialPolyhedron2[2]),
			this.polyhedron._pickVertex().init(initialPolyhedron[3], initialPolyhedron1[3], initialPolyhedron2[3])
		)) {
			GjkEpaLog.log("EPA failed at initialization: " + this.polyhedron._status);
			return GjkEpaResultState.EPA_FAILED_TO_INIT;
		}

		this.simplexSize = 0;
		var supportingVertex: Vec3 = this.s[0];
		var witness1: Vec3 = this.w1[0];
		var witness2: Vec3 = this.w2[0];

		var count: number = 0;
		var maxIterations: number = 40;
		while (count < maxIterations) {
			var face: EpaTriangle = this.polyhedron._getBestTriangle();

			// GjkEpaLog.log("nearest face:");
			// GjkEpaLog.run(face.dump());

			this.dir.copyFrom(face._normal).normalize();
			this.computeSupportingVertex();

			var v0: EpaVertex = face._vertices[0];
			var v1: EpaVertex = face._vertices[1];
			var v2: EpaVertex = face._vertices[2];

			var dot1: number = v0.v.dot(this.dir);
			var dot2: number = supportingVertex.dot(this.dir);

			// GjkEpaLog.log("got new vertex: " + supportingVertex);
			// GjkEpaLog.log("improvement: " + dot1 + " -> " + dot2 + ", normal: " + this.dir.toString());

			if (dot2 - dot1 < 1e-6 || count == maxIterations - 1) { // no improvement
				this.closest.copyFrom(this.dir).scaleEq(this.dir.dot(v0.v) / this.dir.lengthSq());

				var c: Vec3 = new Vec3();
				M.vec3_fromVec3(c, this.closest);
				var s0: Vec3 = new Vec3();
				var w10: Vec3 = new Vec3();
				var w20: Vec3 = new Vec3();
				var s1: Vec3 = new Vec3();
				var w11: Vec3 = new Vec3();
				var w21: Vec3 = new Vec3();
				var s2: Vec3 = new Vec3();
				var w12: Vec3 = new Vec3();
				var w22: Vec3 = new Vec3();
				M.vec3_fromVec3(s0, v0.v); M.vec3_fromVec3(w10, v0.w1); M.vec3_fromVec3(w20, v0.w2);
				M.vec3_fromVec3(s1, v1.v); M.vec3_fromVec3(w11, v1.w1); M.vec3_fromVec3(w21, v1.w2);
				M.vec3_fromVec3(s2, v2.v); M.vec3_fromVec3(w12, v2.w1); M.vec3_fromVec3(w22, v2.w2);

				var s01: Vec3 = new Vec3();
				var s02: Vec3 = new Vec3();
				var s0c: Vec3 = new Vec3();
				M.vec3_sub(s01, s1, s0);
				M.vec3_sub(s02, s2, s0);
				M.vec3_sub(s0c, c, s0);

				var d11: number = M.vec3_dot(s01, s01);
				var d12: number = M.vec3_dot(s01, s02);
				var d22: number = M.vec3_dot(s02, s02);
				var d1c: number = M.vec3_dot(s01, s0c);
				var d2c: number = M.vec3_dot(s02, s0c);
				var invDet: number = d11 * d22 - d12 * d12;
				if (invDet != 0) invDet = 1 / invDet;
				var s: number = (d1c * d22 - d2c * d12) * invDet;
				var t: number = (-d1c * d12 + d2c * d11) * invDet;

				// compute closest points
				var diff: Vec3 = new Vec3();
				var cp1: Vec3 = new Vec3();
				var cp2: Vec3 = new Vec3();

				M.vec3_sub(diff, w11, w10);
				M.vec3_addRhsScaledTo(cp1, w10, diff, s);
				M.vec3_sub(diff, w12, w10);
				M.vec3_addRhsScaledEQ(cp1, cp1, diff, t);

				M.vec3_sub(diff, w21, w20);
				M.vec3_addRhsScaledTo(cp2, w20, diff, s);
				M.vec3_sub(diff, w22, w20);
				M.vec3_addRhsScaledEQ(cp2, cp2, diff, t);

				M.vec3_toVec3(this.closestPoint1, cp1);
				M.vec3_toVec3(this.closestPoint2, cp2);

				this.depth = this.closest.length();
				return GjkEpaResultState.SUCCEEDED;
			}
			var epaVertex: EpaVertex = this.polyhedron._pickVertex().init(supportingVertex, witness1, witness2);
			if (!this.polyhedron._addVertex(epaVertex, face)) {

				// GjkEpaLog.log("EPA failed at vertex addition: " + this.polyhedron._status);
				// GjkEpaLog.run(this.polyhedron._dumpAsObjModel());

				return GjkEpaResultState.EPA_FAILED_TO_ADD_VERTEX;
			}
			count++;
		}

		// GjkEpaLog.log("EPA failed: did not converge.");
		// GjkEpaLog.run(this.polyhedron._dumpAsObjModel());

		return GjkEpaResultState.EPA_DID_NOT_CONVERGE;
	}

	// --- public ---

	/**
	 * Returns an instance of `GjkEpa`.
	 */
	public static getInstance(): GjkEpa {
		return GjkEpa.instance;
	}

	/**
	 * Computes the closest points of two convex geometries `c1` and `c2` with transforms `tf1` and `tf2`
	 * respectively, and returns the status of the result (see `GjkEpaResultState` for details). If cached
	 * data `cache` is not `null`, this tries to exploit the previous result in `cache` to improve performance,
	 * and stores the new result to `cache`.
	 *
	 * Set the compiler option `OIMO_GJK_EPA_DEBUG` for debugging (warning: massive logging).
	 */
	public computeClosestPoints(c1: ConvexGeometry, c2: ConvexGeometry, tf1: Transform, tf2: Transform, cache: CachedDetectorData): number {
		return this.computeClosestPointsImpl(c1, c2, tf1, tf2, cache, true);
	}

	/**
	 * Computes the distance between two convex geometries `c1` and `c2` with transforms `tf1` and `tf2`
	 * respectively, and returns the status of the result (see `GjkEpaResultState` for details). Different
	 * from `GjkEpa.computeClosestPoints`, this does not compute negative distances and closest points if
	 * two geometries are overlapping. If cached data `cache` is not `null`, this tries to exploit the
	 * previous result in `cache` to improve performance, and stores the new result to `cache`.
	 *
	 * Set the compiler option `OIMO_GJK_EPA_DEBUG` for debugging (warning: massive logging).
	 */
	public computeDistance(c1: ConvexGeometry, c2: ConvexGeometry, tf1: Transform, tf2: Transform, cache: CachedDetectorData): number {
		return this.computeClosestPointsImpl(c1, c2, tf1, tf2, cache, false);
	}

	/**
	 * Performs a convex casting between `c1` and `c2`. Returns `true` and sets the result information
	 * to `hit` if the convex geometries intersect. Each convex geometries translates by `tl1` and `tl2`,
	 * starting from the beginning transforms `tf1` and `tf2` respectively.
	 *
	 * Set the compiler option `OIMO_GJK_EPA_DEBUG` for debugging (warning: massive logging).
	 */
	public convexCast(c1: ConvexGeometry, c2: ConvexGeometry, tf1: Transform, tf2: Transform, tl1: Vec3, tl2: Vec3, hit: RayCastHit): boolean {
		return this.convexCastImpl(c1, c2, tf1, tf2, tl1, tl2, hit);
	}

	/**
	 * Performs ray cansting against the convex geometry `c` with transform `tf`. Returns `true` and sets
	 * the result information to `hit` if the line segment from `begin` to `end` intersects the convex
	 * geometry. Otherwise returns `false`.
	 *
	 * Set the compiler option `OIMO_GJK_EPA_DEBUG` for debugging (warning: massive logging).
	 */
	public rayCast(c: ConvexGeometry, tf: Transform, begin: Vec3, end: Vec3, hit: RayCastHit): boolean {
		var tf1: Transform = this.tempTransform;
		var tf2: Transform = tf;

		M.vec3_fromVec3(tf1._position, begin);

		var tl1: Vec3 = this.tl1;
		var tl2: Vec3 = this.tl2;

		tl1.copyFrom(end).subEq(begin);
		tl2.zero();

		return this.convexCastImpl(null, c, tf1, tf2, tl1, tl2, hit);
	}

}
